#include "Formation.h"

namespace Putin{

Formation::Formation()
{
	reservedSlots = 0;
}

/**
* Constructs a new formation with the specified route and 0 reserved slots.
*
* @param r The route this formation will follow.
*/
Formation::Formation(std::vector<Vec3f*> r)
{
	mRoute = r;
	reservedSlots = 0;
}
Formation::~Formation(void)
{
}

/**
* Get the current leg (world coordinate waypoint) for the specified id.
*
* @param id Unit id.
*/
Vec3f Formation::getCurrentLeg(int id)
{
	if(mRoute.empty())
		return *(new Vec3f());

	Vec3f dest = *mRoute.at(mLegs[id]) + getOffset(id);
	return dest;
}

/**
* Get the final destination for this formation's leader. Vec3f(0,0,0) if none exists.
*/
Vec3f Formation::getDestination()
{
	if(mRoute.empty())
		return *(new Vec3f());

	Vec3f dest = *mRoute.back();
	return dest;
}

/**
* Get the final destination for the specified unit id (formation leader pos + offset).
*
* @param id Unit id.
*/
Vec3f Formation::getOffsetDestination(int id)
{
	if(mRoute.empty())
		return *(new Vec3f());

	Vec3f dest = *mRoute.back() + getOffset(id);
	return dest;
}

/**
* Indiciate that a unit has reached their current waypoint, and set the next as active.
*
* @param id Unit id.
* @return returns true if a next waypoint exists, false if the final waypoint has been reached.
*/
bool Formation::nextLeg(int id)
{
	mLegs[id]++;
	if(mLegs[id] < (int) mRoute.size())
		return true;
	else {
		mLegs[id]--;
		return false;
	}
}

/**
* Reserves a slot in the formation for the specified unit id.
*
* @param id Unit id.
* @param unitPos That unit's current position.
*/
void Formation::reserveSlot(int id, Vec3f unitPos)
{
	// if leader, also set final facing
	if(reservedSlots == 0)
	{
		mFinalFacing = (getDestination() - unitPos).normalized();
	}
	mLegs[id] = 0;
	mSlots[id] = reservedSlots;
	reservedSlots++;

}

/**
* Releases a formation slot (not implemented).
*
* @param id Unit id.
*/
void Formation::releaseSlot(int id)
{
	//mSlots.erase(id);
}

/**
* Gets the offset from the leader for the specified unit id.
*
* @param id Unit id.
* @return If found returns the offset, otherwise Vec3f(0,0,0) is returned.
*/
Vec3f Formation::getOffset(int id)
{
	if(mSlots.find(id) == mSlots.end())
		return Vec3f();
	else 
	{
		int index = mSlots[id];
		return Formation::offset(index);
	}
}

/**
* Gets the offset from the leader for the specified slot index.
*
* @param index Slot index.
* @return If index exists returns the offset, otherwise Vec3f(0,0,0) is returned.
*/
Vec3f Formation::offset(int index)
{
	static float dist = 15.0f;
	switch(index)
	{
	case 0:
		return Vec3f(0,0,0);
	case 1:
		return Vec3f(0           ,-1.0f * dist,0);
	case 2:
		return Vec3f(0           , 1.0f * dist,0);
	case 3:
		return Vec3f( 1.0f * dist,           0,0);
	case 4:
		return Vec3f(-1.0f * dist,           0,0);
	case 5:
		return Vec3f( 1.0f * dist, 1.0f * dist,0);
	case 6:
		return Vec3f( 1.0f * dist, -1.0f * dist,0);
	case 7:
		return Vec3f(-1.0f * dist, 1.0f * dist,0);
	case 8:
		return Vec3f(-1.0f * dist,-1.0f * dist,0);
	case 9:
		return Vec3f(0           ,-2.0f * dist,0);
	case 10:
		return Vec3f( 1.0f * dist,-2.0f * dist,0);
	case 11:
		return Vec3f(-1.0f * dist,-2.0f * dist,0);
	case 12:
		return Vec3f( 2.0f * dist,           0,0);
	case 13:
		return Vec3f( 2.0f * dist,-1.0f * dist,0);
	case 14:
		return Vec3f(-2.0f * dist,           0,0);
	case 15:
		return Vec3f(-2.0f * dist,-1.0f * dist,0);
	default:
		return Vec3f(0,0,0);
	}
}

} // end namespace